// The functions contained in this file are pretty dummy
// and are included only as a placeholder. Nevertheless,
// they *will* get included in the shared library if you
// don't remove them :)
//
// Obviously, you 'll have to write yourself the super-duper
// functions to include in the resulting library...
// Also, it's not necessary to write every function in this file.
// Feel free to add more files in this project. They will be
// included in the resulting library.

extern "C++"
{
//
#include"mPoint.h"
double mPoint::GetAngleDiff(mPoint p2,mPoint p3)
{
    double a1 = mP(x,y,z).GetDirectionXY(p2);
    double a2 = p2.GetDirectionXY(p3);
    //printf("%f %f\n",a1,a2);
    double a3 = a2-a1;

    if(a3>PI)a3-=TAU;
    if(a3<(-PI))a3+=TAU;
    return a3;
};

//new point from point XY;
mPoint mPoint::GetFromPoint(double angle,double lenght)
{
    double nx = cos(angle)*lenght;
    double ny = sin(angle)*lenght;
    mPoint T;
    T.SetValue(x+nx,y+ny,z);
    return T;
};
//******************************//
mPoint mPoint::Rotate(double A)
{
    mPoint NP;
    NP.SetValue(  (x*cos(A))-(y*sin(A))  ,  (x*sin(A))+(y*cos(A)) , z);
    return NP;
};
//****************************//
//****************************//
//****************************//
//****************************//
void mPoint::RotateX(double A)
{
    SetValue( mP( x, (y*cos(A))-(z*sin(A))  ,  (y*sin(A))+(z*cos(A)) ) );
};


//****************************//
void mPoint::RotateY(double A)
{
    SetValue( (x*cos(A))-(z*sin(A))  ,  y   ,  (x*sin(A))+(z*cos(A)) );
};

//out
mPoint mPoint::RotateX_out(double A)
{
    mPoint o;
    o.SetValue( mP( x, (y*cos(A))-(z*sin(A))  ,  (y*sin(A))+(z*cos(A)) ) );
    return o;
};


//****************************//
mPoint mPoint::RotateY_out(double A)
{
    mPoint o;
    o.SetValue( (x*cos(A))-(z*sin(A))  ,  y   ,  (x*sin(A))+(z*cos(A)) );
    return o;
};





void mPoint::RotateXY(double A)
{
    double xt = (x*cos(A))-(y*sin(A));
    double yt = (x*sin(A))+(y*cos(A));

    x = xt;
    y = yt;
};
//ympyrän keskipiste XY tasossa
void mPoint::SetCCWCircleCenter(mPoint p1,mPoint p2,double r)
{

    mPoint  T;
    double D2,A;
    double   D = p1.GetDistanceXY(p2)/2;
    D2 = sqrt((r*r)-(D*D));
    if(D2 != D2)D2=0;

    A = p1.GetDirectionXY(p2);
    T = p1.GetMidPoint(p2);

    if(r>0)T = T.GetFromPoint(A+1.570796325,D2);
    if(r<0)T = T.GetFromPoint(A-1.570796325,D2);
    SetValue(T);


}

//ympyrän keskipiste XY tasossa
void mPoint::SetCWCircleCenter(mPoint p1,mPoint p2,double r)
{
    mPoint  T;
    double D2,A;
    double D = p1.GetDistanceXY(p2)/2;
    D2 = sqrt((r*r)-(D*D));
    if(D2 != D2)D2=0;

    A = p1.GetDirectionXY(p2);
    T = p1.GetMidPoint(p2);

    if(r>0)T = T.GetFromPoint(A-1.570796325,D2);
    if(r<0)T = T.GetFromPoint(A+1.570796325,D2);
    SetValue(T);

}
void mPoint::SetValue(double X,double Y,double Z)
{
    x=X;
    y=Y;
    z=Z;
};

void mPoint::SetValue(mPoint v)
{
    x=v.x;
    y=v.y;
    z=v.z;
};

mPoint mPoint::Translate(mPoint P2)
{
    mPoint NP;
    NP.SetValue(x-P2.x,y-P2.y,P2.z);
    return NP;
};

//********************************************************************//
//**                       mPointArray                             **//
//********************************************************************//

//**************************************************************************************************//
//**                                 Functions using mPoint/mPointArray                         **//
//**************************************************************************************************//

//(MATCH)
bool mPoint::Match(mPoint v2,double maxRelativeError)
{
    /*if ((x == t.x) &&  (x == t.x) && (x == t.x)  )return true;
    if ((y == t.y) &&  (y == t.y) && (y == t.y)  )return true;
    if ((z == t.z) &&  (z == t.z) && (z == t.z)  )return true;*/

    double relativeErrorX;
    double relativeErrorY;
    double relativeErrorZ;

    //X
    if (fabs(v2.x) > fabs(x))
        relativeErrorX = fabs(v2.x - x);
    else
        relativeErrorX = fabs(v2.x - x);

    //Y
    if (fabs(v2.y) > fabs(y))
        relativeErrorY = fabs(v2.y - y) ;
    else
        relativeErrorY = fabs(v2.y - y);

    //Z
    if (fabs(v2.z) > fabs(z))
        relativeErrorZ = fabs(v2.z - z);
    else
        relativeErrorZ = fabs(v2.z - z);

    if(relativeErrorX > maxRelativeError)return false;
    if(relativeErrorY > maxRelativeError)return false;
    if(relativeErrorZ > maxRelativeError)return false;

    return true;
};


//
mPoint mP(double x,double y,double z)
{
    mPoint T;
    T.SetValue(x,y,z);
    return T;
};
//
mPoint mPoint::GetMidPoint(mPoint p2,double paikka)
{
    double x1 = (p2.x-x)*paikka+x;
    double y1 = (p2.y-y)*paikka+y;
    double z1 = (p2.z-z)*paikka+z;
    return mP(x1,y1,z1);
};
//direction 0 left  3.14 right...
double mPoint::GetDirectionXY(mPoint p2)
{
    double px = p2.x-x;
    double py = p2.y-y;

    double kulma = atan2(py,px);
    if(kulma<0)kulma+=+6.2831853;
    return kulma;
}

//distance of two points
double mPoint::GetDistanceXY(mPoint p)
{
    double dx = x-p.x;
    double dy = y-p.y;
    double d = sqrt((dx*dx)+(dy*dy));

    return d;
};

double mPoint::GetDistanceXYZ(mPoint p)
{
    double dx = x-p.x;
    double dy = y-p.y;
    double dz = z-p.z;
    double d = sqrt((dx*dx)+(dy*dy)+(dz*dz));

    return d;
}





}
